#include "thread_handling.h"
#include "log.h"
#include <pthread.h>
#include <unistd.h>
#include"../include/my_joystick.h"
#include"../include/uart.h"
#include"../include/type.h"
#include"../include/commands.h"
#include<linux/joystick.h>

// 创建线程的封装函数
int create_thread(pthread_t *thread,void*(*function)(void*),void* arg)
{
    int ret=pthread_create(thread,NULL,function,arg);
    if(ret!=0){
        LOG_ERR("Failed to create thread: %d", ret);
        return -1;
    }
    LOG_INFO("Thread created successfully");
    return 0;
}
void destroy_thread(pthread_t thread)
{
    int ret = pthread_join(thread, NULL);
    if (ret != 0) {
        LOG_ERR("Failed to join thread: %d", ret);
    } else {
        LOG_INFO("Thread joined successfully");
    }
}

void* pthread_addVal(void * arg)
{
    while(1){
        if(trans_flag){
            if(motor_add_flag == 1){
                motor_speed += 100;
                if(motor_speed > MOTOR_SPEED_MAX){
                    motor_speed = MOTOR_SPEED_MAX;
                }
                host_ctr_cmd_g.motor_ctr = Speed_ClosedLoop_ctr(motor_speed);
                if(trans_flag){
                    uart_send_command(&host_ctr_cmd_g,sizeof(host_ctr_cmd_g));
                    usleep(200*1000);
                }
            }
            if(motor_add_flag == 2){
            motor_speed -= 100;
            if(motor_speed < 0){
                motor_speed = 0;
            }
            host_ctr_cmd_g.motor_ctr = Speed_ClosedLoop_ctr(motor_speed);
            if(trans_flag){
               uart_send_command(&host_ctr_cmd_g,sizeof(host_ctr_cmd_g));
               sleep(200);
            }
            }

            if(stepmotor_add_flag == 1){
            host_ctr_cmd_g.stepmotor_ctr.dist++;
            if(host_ctr_cmd_g.stepmotor_ctr.dist > STEPMOTOR_MAXDIST){
                host_ctr_cmd_g.stepmotor_ctr.dist = STEPMOTOR_MAXDIST;
            }
            if(trans_flag){
               uart_send_command(&host_ctr_cmd_g,sizeof(host_ctr_cmd_g));
            }
            }   
            if(stepmotor_add_flag == 2){
             host_ctr_cmd_g.stepmotor_ctr.dist--;
            if(host_ctr_cmd_g.stepmotor_ctr.dist < 0 ){
                host_ctr_cmd_g.stepmotor_ctr.dist = 0;
            }
            if(trans_flag){
                uart_send_command(&host_ctr_cmd_g,sizeof(host_ctr_cmd_g));
            }
            }

            if(headx_add_flag == 1){
            host_ctr_cmd_g.servo_ctr.servo_head_x+=5;
            if(host_ctr_cmd_g.servo_ctr.servo_head_x > SERVO_HEAD_MAX){
                host_ctr_cmd_g.servo_ctr.servo_head_x = SERVO_HEAD_MAX;
            }
            if(trans_flag){
                uart_send_command(&host_ctr_cmd_g,sizeof(host_ctr_cmd_g));
            }
            }
            if(headx_add_flag == 2){
            host_ctr_cmd_g.servo_ctr.servo_head_x-=5;
            if(host_ctr_cmd_g.servo_ctr.servo_head_x < SERVO_HEAD_MIN){
                host_ctr_cmd_g.servo_ctr.servo_head_x = SERVO_HEAD_MIN;
            }
            if(trans_flag){
              uart_send_command(&host_ctr_cmd_g,sizeof(host_ctr_cmd_g));
            }
            }

            if(heady_add_flag == 1){
            host_ctr_cmd_g.servo_ctr.servo_head_y+=5;
            if(host_ctr_cmd_g.servo_ctr.servo_head_y > SERVO_HEAD_MAX){
                host_ctr_cmd_g.servo_ctr.servo_head_y = SERVO_HEAD_MAX;
            }
            if(trans_flag){
                uart_send_command(&host_ctr_cmd_g,sizeof(host_ctr_cmd_g));
            }
            }
            if(heady_add_flag == 2){
            host_ctr_cmd_g.servo_ctr.servo_head_y-=5;
            if(host_ctr_cmd_g.servo_ctr.servo_head_y < SERVO_HEAD_MIN){
                host_ctr_cmd_g.servo_ctr.servo_head_y = SERVO_HEAD_MIN;
            }
            if(trans_flag){
                uart_send_command(&host_ctr_cmd_g,sizeof(host_ctr_cmd_g));
            }
            }

            if(fin_left_out_add_flag == 1){
            host_ctr_cmd_g.servo_ctr.servo_fin_left_out+=10;
            if(host_ctr_cmd_g.servo_ctr.servo_fin_left_out > SERVO_FIN_MAX){
                host_ctr_cmd_g.servo_ctr.servo_fin_left_out = SERVO_FIN_MAX;
            }
            if(trans_flag){
               uart_send_command(&host_ctr_cmd_g,sizeof(host_ctr_cmd_g));
            }
            }
            if(fin_left_out_add_flag == 2){
            host_ctr_cmd_g.servo_ctr.servo_fin_left_out-=10;
            if(host_ctr_cmd_g.servo_ctr.servo_fin_left_out < SERVO_FIN_MIN){
                host_ctr_cmd_g.servo_ctr.servo_fin_left_out = SERVO_FIN_MIN;
            }
            if(trans_flag){
               uart_send_command(&host_ctr_cmd_g,sizeof(host_ctr_cmd_g));
            }
            }

            if(fin_left_in_add_flag == 1){
            host_ctr_cmd_g.servo_ctr.servo_fin_left_in+=10;
            if(host_ctr_cmd_g.servo_ctr.servo_fin_left_in > SERVO_FIN_MAX){
                host_ctr_cmd_g.servo_ctr.servo_fin_left_in = SERVO_FIN_MAX;
            }
            if(trans_flag){
               uart_send_command(&host_ctr_cmd_g,sizeof(host_ctr_cmd_g));
            }
            }
            if(fin_left_in_add_flag == 2){
            host_ctr_cmd_g.servo_ctr.servo_fin_left_in-=10;
            if(host_ctr_cmd_g.servo_ctr.servo_fin_left_in < SERVO_FIN_MIN){
                host_ctr_cmd_g.servo_ctr.servo_fin_left_in = SERVO_FIN_MIN;
            }
            if(trans_flag){
                uart_send_command(&host_ctr_cmd_g,sizeof(host_ctr_cmd_g));
            }
            } 

            if(fin_right_in_add_flag == 1){
            host_ctr_cmd_g.servo_ctr.servo_fin_right_in+=10;
            if(host_ctr_cmd_g.servo_ctr.servo_fin_right_in > SERVO_FIN_MAX){
                host_ctr_cmd_g.servo_ctr.servo_fin_right_in = SERVO_FIN_MAX;
            }
            if(trans_flag){
               uart_send_command(&host_ctr_cmd_g,sizeof(host_ctr_cmd_g));
            }
            }
            if(fin_right_in_add_flag == 2){
            host_ctr_cmd_g.servo_ctr.servo_fin_right_in-=10;
            if(host_ctr_cmd_g.servo_ctr.servo_fin_right_in < SERVO_FIN_MIN){
                host_ctr_cmd_g.servo_ctr.servo_fin_right_in = SERVO_FIN_MIN;
            }
            if(trans_flag){
               uart_send_command(&host_ctr_cmd_g,sizeof(host_ctr_cmd_g));
            }
            }

            if(fin_right_out_add_flag == 1){
            host_ctr_cmd_g.servo_ctr.servo_fin_right_out+=10;
            if(host_ctr_cmd_g.servo_ctr.servo_fin_right_out > SERVO_FIN_MAX){
                host_ctr_cmd_g.servo_ctr.servo_fin_right_out = SERVO_FIN_MAX;
            }
            if(trans_flag){
              uart_send_command(&host_ctr_cmd_g,sizeof(host_ctr_cmd_g));
            }
            }
            if(fin_right_out_add_flag == 2){
            host_ctr_cmd_g.servo_ctr.servo_fin_right_out-=10;
            if(host_ctr_cmd_g.servo_ctr.servo_fin_right_out < SERVO_FIN_MIN){
                host_ctr_cmd_g.servo_ctr.servo_fin_right_out = SERVO_FIN_MIN;
            }
            if(trans_flag){
               uart_send_command(&host_ctr_cmd_g,sizeof(host_ctr_cmd_g));
            }
            }
        }
        usleep(150*1000);
    }
}
void* pthread_joystick(void * arg)//void* arg 是一个通用指针，用于传递任意类型的数据给线程函数。
{
    int fd = *(int *)arg;  // 操纵杆设备文件描述符
    struct js_event jse;

    while (1) {  // 无限循环，不断读取操纵杆事件
        if (joystick_read_ready(fd)) {
            if (joystick_read_one_event(fd, &jse) == sizeof(jse)) {
                if ((jse.type & ~JS_EVENT_INIT) == JS_EVENT_BUTTON) {
                    process_button_event(jse.number, jse.value);
                } else if (jse.type == JS_EVENT_AXIS) {
                    process_axis_event(&jse);
                }
            }
        }
        usleep(2000);  // 睡眠 2 毫秒
    }

    joystick_close(fd);
    return NULL;




}